/*
 * contactSolver.cpp
 *
 *  Created on: Mar 11, 2012
 *      Author: Michael Ziminsky
 */

bool ContactSolver::handleCollisionIfAny(RigidBody &a, RigidBody &b)
{
    bool collide = false;
    if (a._mass + b._mass > .0001)
    try
    {
        const ConvexShape &shapeA = dynamic_cast<ConvexShape&>(a);
        const ConvexShape &shapeB = dynamic_cast<ConvexShape&>(b);
        vector3 norm;
        scalar depth;
        collide = intersect(shapeA, shapeB, norm, depth, (a.position() - b.position()));
        /**
         * Handle collision: apply impulse to bodies
         */
        if (collide)
        {
//            vector3 impulse = (1.00001 * norm * arma::dot((a._velocity - b._velocity), norm)) * ((a._mass * b._mass) / (a._mass + b._mass));
            vector3 impulse = (1.00001 * norm * arma::dot((a._velocity - b._velocity), norm)) / (a.invMass() + b.invMass());

            vector3 dist = depth * norm;
            if (a._mass) a.position(a._position - dist);
            if (b._mass) b.position(b._position + dist);

            a._velocity -= (impulse * a.invMass()) * (1 + a._elastic);
            b._velocity += (impulse * b.invMass()) * (1 + b._elastic);

        }
    }
    catch (...)
    {
    }
    return collide;
}
